#include <ServoSoft.h>
#include <MsTimer2.h>
#include <defArduinoParaArduino.h>

#define TEMPO 600

// O da esquerda eh o "mole"
#define Esq_FT 0 // controle da esquerda, pot frente-tras na analog 0
#define Esq_DE 2
#define Dir_FT 1
#define Dir_DE 3


// Valores analogicos do joystick

// controle esquerdo
#define ce_frente 411
#define ce_centroFT 724
#define ce_tras 840

#define ce_esq 487
#define ce_centroDE 761
#define ce_dir 856

//controle direito

#define cd_frente 847
#define cd_centroFT 757
#define cd_tras 190

#define cd_esq 260
#define cd_centroDE 690
#define cd_dir 838


// Ma - Frente - Direita
#define Ma_b1 6 // fio branco
#define Ma_b2 7 // fio laranja

// Mb - Frente - Esquerda
#define Mb_b1 8 // marrom escuro
#define Mb_b2 9 // marrom claro

// Mc - Tras - Direita
#define Mc_b1 11 // verde claro
#define Mc_b2 10 // verde escuro

// Md - Tras - Esquerda
#define Md_b1 13 // fio azul claro
#define Md_b2 12 //fio azul escuro

 Servo servo01; // frontal - direita
 Servo servo02; // frontal - esquerda
 Servo servo03; // traseira - esquerda
 Servo servo04; // traseira - direita
 
 Servo servo; // para testes
 
 
void refreshServos()
{
  Servo::refresh();  
}

void avante()
{
    servo01.write(90);
    servo02.write(0);
    servo03.write(90);
    servo04.write(0);
    Servo::refresh();

    delay(700);    
}

void aoLado()
{
    servo01.write(0);
    servo02.write(90);
    servo03.write(0);
    servo04.write(90);
    Servo::refresh();    

    delay(700);
}

void girar()
{
    servo01.write(45);
    servo02.write(45);
    servo03.write(45);
    servo04.write(45);
    Servo::refresh();    
    
    delay(500);
}

void stop()
{
  digitalWrite(Ma_b1,LOW);
  digitalWrite(Ma_b2,LOW);
  
  digitalWrite(Mb_b1,LOW);
  digitalWrite(Mb_b2,LOW);

  digitalWrite(Mc_b1,LOW);
  digitalWrite(Mc_b2,LOW);
  
  digitalWrite(Md_b1,LOW);
  digitalWrite(Md_b2,LOW);
  
  delay(500);
}

void go_fwd(long int time)
{
  avante();
  
  digitalWrite(Ma_b1,HIGH);
  digitalWrite(Ma_b2,LOW);
  
  digitalWrite(Mb_b1,HIGH);
  digitalWrite(Mb_b2,LOW);

  digitalWrite(Mc_b1,HIGH);
  digitalWrite(Mc_b2,LOW);
  
  digitalWrite(Md_b1,HIGH);
  digitalWrite(Md_b2,LOW);
  
  delay(time);
  stop();
}


void go_fwd2( )
{
  avante();
  
  digitalWrite(Ma_b1,HIGH);
  digitalWrite(Ma_b2,LOW);
  
  digitalWrite(Mb_b1,HIGH);
  digitalWrite(Mb_b2,LOW);

  digitalWrite(Mc_b1,HIGH);
  digitalWrite(Mc_b2,LOW);
  
  digitalWrite(Md_b1,HIGH);
  digitalWrite(Md_b2,LOW);
  
//  delay(time);
 // stop();
}


void go_rev(long int time)
{
  avante();
  
  digitalWrite(Ma_b1,LOW);
  digitalWrite(Ma_b2,HIGH);
  
  digitalWrite(Mb_b1,LOW);
  digitalWrite(Mb_b2,HIGH);

  digitalWrite(Mc_b1,LOW);
  digitalWrite(Mc_b2,HIGH);
  
  digitalWrite(Md_b1,LOW);
  digitalWrite(Md_b2,HIGH);
  
  
  delay(time);
  stop();
}


void go_rev2( )
{
  avante();
  
  digitalWrite(Ma_b1,LOW);
  digitalWrite(Ma_b2,HIGH);
  
  digitalWrite(Mb_b1,LOW);
  digitalWrite(Mb_b2,HIGH);

  digitalWrite(Mc_b1,LOW);
  digitalWrite(Mc_b2,HIGH);
  
  digitalWrite(Md_b1,LOW);
  digitalWrite(Md_b2,HIGH);
  
  
  //delay(time);
  //stop();
}

void go_right(long int time)
{
  aoLado();
  
  digitalWrite(Ma_b1,LOW);
  digitalWrite(Ma_b2,HIGH);
  
  digitalWrite(Mb_b1,HIGH);
  digitalWrite(Mb_b2,LOW);

  digitalWrite(Mc_b1,HIGH);
  digitalWrite(Mc_b2,LOW);
  
  digitalWrite(Md_b1,LOW);
  digitalWrite(Md_b2,HIGH);
  
  delay(time);
  stop();
}


void go_right2( )
{
  aoLado();
  
  digitalWrite(Ma_b1,LOW);
  digitalWrite(Ma_b2,HIGH);
  
  digitalWrite(Mb_b1,HIGH);
  digitalWrite(Mb_b2,LOW);

  digitalWrite(Mc_b1,HIGH);
  digitalWrite(Mc_b2,LOW);
  
  digitalWrite(Md_b1,LOW);
  digitalWrite(Md_b2,HIGH);
  
  //delay(time);
  //stop();
}

void go_left(long int time)
{
  aoLado();
  
  digitalWrite(Ma_b1,HIGH);
  digitalWrite(Ma_b2,LOW);
  
  digitalWrite(Mb_b1,LOW);
  digitalWrite(Mb_b2,HIGH);

  digitalWrite(Mc_b1,LOW);
  digitalWrite(Mc_b2,HIGH);
  
  digitalWrite(Md_b1,HIGH);
  digitalWrite(Md_b2,LOW);
  
  delay(time);
  stop();
}


void go_left2( )
{
  aoLado();
  
  digitalWrite(Ma_b1,HIGH);
  digitalWrite(Ma_b2,LOW);
  
  digitalWrite(Mb_b1,LOW);
  digitalWrite(Mb_b2,HIGH);

  digitalWrite(Mc_b1,LOW);
  digitalWrite(Mc_b2,HIGH);
  
  digitalWrite(Md_b1,HIGH);
  digitalWrite(Md_b2,LOW);
  
 // delay(time);
 // stop();
}

void turn_ccw(long int time)
{
  girar();
  
  digitalWrite(Ma_b1,HIGH);
  digitalWrite(Ma_b2,LOW);
  
  digitalWrite(Mb_b1,LOW);
  digitalWrite(Mb_b2,HIGH);

  digitalWrite(Mc_b1,HIGH);
  digitalWrite(Mc_b2,LOW);
  
  digitalWrite(Md_b1,LOW);
  digitalWrite(Md_b2,HIGH);
  
  delay(time);
  stop();
}

void turn_cw(long int time)
{
  girar();
  
  digitalWrite(Ma_b1,LOW);
  digitalWrite(Ma_b2,HIGH);
  
  digitalWrite(Mb_b1,HIGH);
  digitalWrite(Mb_b2,LOW);

  digitalWrite(Mc_b1,LOW);
  digitalWrite(Mc_b2,HIGH);
  
  digitalWrite(Md_b1,HIGH);
  digitalWrite(Md_b2,LOW);
  
  delay(time);
  stop();
}

void setup() 
{ 
  
  MsTimer2::set(20,refreshServos);
  Serial.begin(19200);
  
  pinMode(Ma_b1,OUTPUT);
  pinMode(Ma_b2,OUTPUT);
  
  pinMode(Mb_b1,OUTPUT);
  pinMode(Mb_b2,OUTPUT);
  
  pinMode(Mc_b1,OUTPUT);
  pinMode(Mc_b2,OUTPUT);
  
  pinMode(Md_b1,OUTPUT);
  pinMode(Md_b2,OUTPUT);
  
  /* --- Configuracao para os servos MG995 
  
  servo01.attach(9); 
  servo01.setMinimumPulse(880);
  servo01.setMaximumPulse(2840);
  
  servo02.attach(10);
  servo02.setMinimumPulse(900);
  servo02.setMaximumPulse(2860);
  
  servo03.attach(11);
  servo03.setMinimumPulse(880);
  servo03.setMaximumPulse(2790);
    
  servo04.attach(12);
  servo04.setMinimumPulse(770);
  servo04.setMaximumPulse(2700);
   
*/

/* --- Configuracao para os servos MG946R */

  servo01.attach(2); 
  servo01.setMinimumPulse(840);
  servo01.setMaximumPulse(2680);

  servo02.attach(3); 
  servo02.setMinimumPulse(800);
  servo02.setMaximumPulse(2680);

  servo03.attach(4); 
  servo03.setMinimumPulse(900);
  servo03.setMaximumPulse(2830);
  
  servo04.attach(5); 
  servo04.setMinimumPulse(880);
  servo04.setMaximumPulse(2830);
  
  
  MsTimer2::start();
  
  avante();
} 
 

void demonstracao()
{
    go_fwd(1500);
    go_right(1500);
    go_rev(1500);
    go_left(750);
    go_fwd(750);
    turn_ccw(500);
    go_fwd(500);
    turn_cw(1000);
    go_fwd(500);
    go_left(500);
    go_rev(500);
    go_right(00);
    turn_ccw(2000);
}

void loop() 
{  
    int v = 0;
    char ch;
       
      
  int valorCE_FT = 0;
  int valorCE_DE = 0;
  int valorCD_FT = 0;
  int valorCD_DE = 0;
  
  // esses eps tem q ser melhor estimados
  int eps1 = 50; 
  int eps2 = 50;
  int eps3 = 50;
  int eps4 = 50;
  
  valorCE_FT = analogRead(Esq_FT);
  valorCE_DE = analogRead(Esq_DE);
  valorCD_FT = analogRead(Dir_FT);
  valorCD_DE = analogRead(Dir_DE);
  
  
  
  if( (valorCE_FT < ce_frente + eps1) && (valorCE_FT > ce_frente - eps1) )
    go_fwd2();
    //Serial.println("frente");
  else
    if( (valorCE_FT < ce_tras + eps2) && (valorCE_FT > ce_tras - eps2) )
      go_rev2();
      //Serial.println("tras");
    else  
      if( (valorCE_DE < ce_dir + eps3) && (valorCE_DE > ce_dir - eps3) )
        go_right2();
        //Serial.println("direita");
    else
      if( (valorCE_DE < ce_esq + eps4) && (valorCE_DE > ce_esq - eps4) )
        go_left2();
        //Serial.println("esquerda");
      else
        stop();
  
  delay(100);
  
  /*  while( Serial.available() )
    { 
      
      switch( ch = Serial.read() )
      {
        case '0': v = 10*v + ch - '0';
                  break;
        
        case '1': v = 10*v + ch - '0';
                  break;
        
        case '2': v = 10*v + ch - '0';
                  break;
        
        case '3': v = 10*v + ch - '0';
                  break;
        
        case '4': v = 10*v + ch - '0';
                  break;
        
        case '5': v = 10*v + ch - '0';
                  break;
        
        case '6': v = 10*v + ch - '0';
                  break;
        
        case '7': v = 10*v + ch - '0';
                  break;
                  
        case '8': v = 10*v + ch - '0';
                  break;
               
        case '9': v = 10*v + ch - '0';
                  break;
      
        case 'w': go_fwd(TEMPO);
                  break;
        
        case 'd': go_right(TEMPO);
                  break;
                  
        case 'a': go_left(TEMPO);
                  break;
                  
        case 's': go_rev(TEMPO);
                  break;
                  
        case 'f': avante();
                  break;
                  
        case 'l': aoLado();
                  break;
                  
        case 'g': girar();
                  turn_ccw(TEMPO);
                  avante();
                  break;
                  
       case 'h': girar();
                 turn_cw(TEMPO);
                 avante();
                 break;

       default : break;           
      
      }
      
    delay(50); 
  }*/
    
} 
